#pragma once
#include "ti_msp_dl_config.h"
#include "Jy61p.h"
#include "Huiduo.h"
#include "Motor.h"
// 定义积分限幅值
#define INTEGRAL_LIMIT 10000.0f
// 灰度传感器数量
#define GRAY_SENSOR_COUNT 8

// PID状态结构体
typedef struct {
    float error;      // 当前误差
    float prev_error; // 上一次误差
    float integral;   // 积分项
    float derivative; // 微分项
} PID_State;

// PID参数结构体
typedef struct {
    // 角度环参数
    float Median;       // 角度目标值
    float Bp, Bi, Bd;   // 角度环PID参数  
    // 速度环参数
    float Target_Speed; // 速度目标值
    float Sp_L, Si_L, Sd_L; // 左轮速度环PID参数
    float Sp_R, Si_R, Sd_R; // 右轮速度环PID参数
    // 转向环参数
    float Tp, Ti, Td;   // 转向环PID参数
    
} PID_Params;
// 全局变量声明

extern PID_Params PID;
extern PID_State angle_state;
extern PID_State speed_state_l;
extern PID_State speed_state_r;
extern PID_State turn_state;

// 灰度传感器数据声明
extern float GraySensors[GRAY_SENSOR_COUNT];

// 函数声明
float Angle_PID(void);
float SpeedL_Pwm(void);
float SpeedR_Pwm(void);
float Turn_PID(void);  // 转向环函数
void Get_Pid(float Bp, float Bd, float Bi,float Target_Speed,
             float Sp_L, float Sd_L, float Si_L,
             float Sp_R, float Sd_R, float Si_R,
             float Tp, float Ti, float Td);  // 添加转向环参数

// 内联PID计算函数
static inline float Calculate_PID(float target, float current, 
                                 PID_State *state, 
                                 float kp, float ki, float kd) 
{
    // 计算误差
    float error = target - current;
    
    // 角度环误差限幅，确保误差不超过正负50
    if (state == &angle_state) {
        if (error > 50.0f) {
            error = 50.0f;
        } else if (error < -50.0f) {
            error = -50.0f;
        }
    }
    
    // 更新积分项并限幅
    float integral = state->integral + error;
    integral = (integral > INTEGRAL_LIMIT) ? INTEGRAL_LIMIT : 
              ((integral < -INTEGRAL_LIMIT) ? -INTEGRAL_LIMIT : integral);
    
    // 计算微分项
    const float derivative = error - state->prev_error;
    
    // 更新状态
    state->error = error;
    state->prev_error = error;
    state->integral = integral;
    state->derivative = derivative;
    
    // 计算并返回输出
    return (kp * error) + (ki * integral) + (kd * derivative);
}

